/******************************* 状态转移图 **********************************
 * 
 * 
 * 
INITILIZED SYSTEM ---> AUTO.TAKEOFF----> HOLD(POSITION)------> OFFBOARD
        ^                                 |   | ^    ^             |
        |                                 |   | |    |-------------|
        |                                 |   | |
        |                                 |   V |
        |<------------ AUTO.LAND <--------| MISSION
        |                                 |
        |                                 |
        |<------------ AUTO.RETURN <------|
 *
 *
 * 
***************************************************************************/

#include "flight_handler.h"

const char SPLIT_CHAR = ' ';

FlightHandler::FlightHandler()
    : nh()
{
    inputCmdSub = nh.subscribe<std_msgs::String>(MAVPROXY_INPUT_CMD, 1000, std::bind(&FlightHandler::CmdCallback, this, std::placeholders::_1));
    flightStateSub = nh.subscribe<mavproxy_ros1::FlightState>(MAVPROXY_FLIGHT_STATE, 1000, std::bind(&FlightHandler::StateCallback, this, std::placeholders::_1));
    parsedCmdPub = nh.advertise<mavproxy_ros1::FlightCmd>(MAVPROXY_PARSED_CMD, GLOBAL_DEFAULT_QUEUE_SIZE);
    // controllClient = nh.serviceClient<mavproxy_ros1::FlightControll>(MAVPROXY_CONTROLL);  

}

// 命令解析
mavproxy_ros1::FlightCmd FlightHandler::GetFlightCmd(const std::string &str)
{
    std::vector<std::string> paramVector;
    mavproxy_ros1::FlightCmd taskCmd;

    int start = 0;
    for (int end = 0; end < str.size(); end++)
    {
        if (str[end] == SPLIT_CHAR)
        {
            std::string temp = str.substr(start, end - start);
            paramVector.push_back(temp);
            start = end + 1;
        }
        if (end == str.size() - 1 && start <= end)
        {
            std::string temp = str.substr(start, end - start + 1);
            paramVector.push_back(temp);
        }
    }

    taskCmd.cmdid = 0;
    taskCmd.cmdtype = paramVector[0];
    for (int i = 1; i < paramVector.size(); i++)
    {
        ROS_INFO("Split the flight cmd param [%s]", paramVector[i].c_str());
        int temp = std::stoi(paramVector[i].c_str());
        taskCmd.paramdata.push_back(temp);
    }
    return taskCmd;
}

// 收到一个新命令，handler应该执行3步操作
// 1、解析字符串成为标准指令
// 2、根据下发的指令触发状态机的转移和操作执行
// 3、轮询传感器、惯导数据、飞机状态，判断任务是否完成
void FlightHandler::CmdCallback(const std_msgs::String::ConstPtr &msg)
{
    // 第一步
    cmd = GetFlightCmd(msg->data);

    // 第二步
    switch (state.droneState)
    {
    case DroneState::STABLIZE:
        // if (cmd.cmdtype == TAKEOFF){
        //     // 下发解锁指令
        //     mavproxy_ros1::FlightControll controll_srv;
        //     controll_srv.request.cmdtype=CMD_FLIGHT_ARM;
        //     controllClient.call(controll_srv);
            
        //     // 切换状态至TAKEOFF
        // }
        break;
    case DroneState::TAKEOFF:
        break;
    case DroneState::LAND:
        break;
    case DroneState::RETURN:
        break;
    case DroneState::HOLD:
        break;
    default:
        break;
    }
}

void FlightHandler::StateCallback(const mavproxy_ros1::FlightState::ConstPtr &msg)
{
    state = *msg;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "flight_handler");
    FlightHandler node;
    ros::spin();
    return RETSUCCESS;
}